uint8_t PWM_PIN[8] = {0,1,2,3,4,5,6,7};



void writeServos() {
    OCR1A = servo[0];
    OCR1B = servo[1];
    OCR3A = servo[2];
}

void initServos(){
    TCCR1A |= (1<<WGM11); // phase correct mode & prescaler to 8
    TCCR1A &= ~(1<<WGM10);
    TCCR1B &= ~(1<<WGM12) &  ~(1<<CS10) & ~(1<<CS12);
    TCCR1B |= (1<<WGM13) | (1<<CS11);
    #if defined(SERVO_RFR_50HZ) 
      ICR1   |= 16700; // TOP to 16700; 
    #endif
    #if defined(SERVO_RFR_160HZ) 
      ICR1   |= 6200; // TOP to 6200; 
    #endif
    #if defined(SERVO_RFR_300HZ) 
      ICR1   |= 3330; // TOP to 3330;  
    #endif
    
    TCCR3A |= (1<<WGM31); // phase correct mode & prescaler to 8
    TCCR3A &= ~(1<<WGM30);
    TCCR3B &= ~(1<<WGM32) &  ~(1<<CS30) & ~(1<<CS32);
    TCCR3B |= (1<<WGM33) | (1<<CS31);
    #if defined(SERVO_RFR_50HZ) 
      ICR3   |= 16700; // TOP to 16700; 
    #endif
    #if defined(SERVO_RFR_160HZ) 
      ICR3   |= 6200; // TOP to 6200; 
    #endif
    #if defined(SERVO_RFR_300HZ) 
      ICR3   |= 3330; // TOP to 3330;  
    #endif    
    
    
    pinMode(9,OUTPUT);
    TCCR1A |= (1<<COM1A1); // pin 9
    OCR1A = TILT_PITCH_MIDDLE;
    pinMode(10,OUTPUT);
    OCR1B = TILT_ROLL_MIDDLE;
    TCCR1A |= (1<<COM1B1); // pin 10
    pinMode(5,OUTPUT);
    OCR3A = 1500;
    TCCR3A |= (1<<COM3A1); // pin 5
}

/**************************************************************************************/
/**********    Mixes the Computed stabilize values to the Servos        ***************/
/**************************************************************************************/
void mixTable() {
    servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] /16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
    servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP   * angle[ROLL]  /16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
    servo[2] = constrain(rcCommand[YAW]+1500, TILT_PAN_MIN, TILT_PAN_MAX);
    debug[0] = servo[2];
    writeServos();
}
